#include <math.h>
#include "motor_drv.h"
#include"ini.h"
#include<iostream>
#include"main.hpp"
// # define INFOHEADER
MotorDrv::MotorDrv()
{
    ;
}

int MotorDrv::init(char *dev, MotorAttr_t *attr)
{
    canId = 0x601;
    memcpy(&tMotorAttr, attr, sizeof(MotorAttr_t));
    return can.init(dev);
}

void MotorDrv::exit()
{
    unsigned char aControlWord[] = {0x2B, 0x40, 0x60, 0, 6, 0, 0, 0};
    setMotorInfo(canId, aControlWord, sizeof(aControlWord));
    can.exit();
}

int MotorDrv::setMotorInfo(unsigned int id, unsigned char *pData, unsigned char len)
{
    struct can_frame frame;
    can.clearBuf();
    can.send(id, pData, len);
    can.getRecive(&frame);
    if((frame.can_id & 0x7F) == (id & 0x7F))
    {
        if(frame.data[0] == 0x60)
        {
            return 0;
        }
        else if(frame.data[0] == 0x80)
        {
            return -1;
        }
        else
        {
            return -1;
        }
    }

    return -1;
}

int MotorDrv::getMotorInfo(unsigned int id, unsigned char *pData, unsigned char len, int *output)
{
    int ret = 0;
    struct can_frame frame;
    can.clearBuf();
    can.send(id, pData, len);
    can.getRecive(&frame);
    if((frame.can_id & 0x7F) == (id & 0x7F))
    {
        if((frame.data[0] & 0xF) == 0x3)       //4字节
        {
            ret = frame.data[7];
            ret <<= 8;
            ret |= frame.data[6];
            ret <<= 8;
            ret |= frame.data[5];
            ret <<= 8;
            ret |= frame.data[4];

            *output = ret;
            return 0;
        }
        else if((frame.data[0] & 0xF) == 0x7)      //3字节
        {
            ret = frame.data[6];
            ret <<= 8;
            ret |= frame.data[5];
            ret <<= 8;
            ret |= frame.data[4];

            if(frame.data[6] >= 0x80)
            {
                ret |= 0xFF000000;
            }
            
            *output = ret;
            return 0;
        }
        else if((frame.data[0] & 0xF) == 0xB)      //2字节
        {
            ret = frame.data[5];
            ret <<= 8;
            ret |= frame.data[4];

            if(frame.data[5] >= 0x80)
            {
                ret |= 0xFFFF0000;
            }

            *output = ret;
            return 0;
        }
        else if((frame.data[0] & 0xF) == 0xF)
        {
            *output = frame.data[4];
            if(frame.data[4] >= 0x80)
            {
                *output |= 0xFFFFFF00;
            }
            return 0;
        } 
        else
        {
            return -1;
        }
    }

    return -1;
}

CIA402_STATE_e MotorDrv::getState(unsigned short param)
{
    unsigned char value, HighHalfByte;
    HighHalfByte = (param >> 5) & 0x3;
    value = (HighHalfByte << 4) | (param & 0xF);
    if((value == 0) || (value == 0x10))
    {
        return CIA_NOT_READY_SWITCH_ON;
    }
    else if((value == 0x20) || (value == 0x30))
    {
        return CIA_SWITCH_ON_DISABLE;
    }
    else if(value == 0x11)
    {
        return CIA_READY_SWITCH_ON;
    }
    else if(value == 0x13)
    {
        return CIA_SWITCH_ON;
    }
    else if(value == 0x17)
    {
        return CIA_OPERATION_ENABLE;
    }
    else if(value == 0x07)
    {
        return CIA_QUCIK_STOP;
    }
    else if((value == 0xF) || (value == 0x1F))
    {
        return CIA_FAULT_REACTION;
    }
    else if((value == 0x08) || (value == 0x18))
    {
        return CIA_FAULT;
    }
}

int MotorDrv::changeState(CIA402_STATE_e curState, CIA402_STATE_e targetState)
{
    unsigned char aStateWord[] = {0x40, 0x41, 0x60, 0, 0, 0, 0, 0};
    unsigned char aReadControlWord[] = {0x40, 0x40, 0x60, 0, 0, 0, 0, 0};
    unsigned char aControlWord[] = {0x2B, 0x40, 0x60, 0, 0, 0, 0, 0};
    unsigned short hControlWord = 0xFF;
    int temp;
    int ret;

    for(int i = 0; i < 20; i++)
    {
        if(curState == targetState)
            return 0;

        switch(curState)
        {
        case CIA_NOT_READY_SWITCH_ON:       //该状态会自动跳转，等待即可
            break;

        case CIA_SWITCH_ON_DISABLE:
            if(targetState == CIA_NOT_READY_SWITCH_ON)      //无法往上跳转，没有跳转路径
                return -1;

            switch(targetState)
            {
            case CIA_READY_SWITCH_ON:
            case CIA_SWITCH_ON:
            case CIA_OPERATION_ENABLE:
            case CIA_QUCIK_STOP:
                aControlWord[4] = 0x6;
                break;
            
            default:                //其他目标状态不允许跳转
                return -1;
                break;
            }
            break;

        case CIA_READY_SWITCH_ON:
            switch(targetState)
            {
            case CIA_SWITCH_ON:
            case CIA_OPERATION_ENABLE:
            case CIA_QUCIK_STOP:
                aControlWord[4] = 0x7;
                break;

            case CIA_SWITCH_ON_DISABLE:
                aControlWord[4] = 0x2;
                break;

            default:                //其他目标状态不允许跳转
                return -1;
                break;
            }
            break;

        case CIA_SWITCH_ON:
            switch(targetState)
            {
            case CIA_OPERATION_ENABLE:
            case CIA_QUCIK_STOP:
                aControlWord[4] = 0xF;
                
                break;

            case CIA_SWITCH_ON_DISABLE:
                aControlWord[4] = 0x2;
                break;

            case CIA_READY_SWITCH_ON:
                aControlWord[4] = 0x6;
                break;

            default:                //其他目标状态不允许跳转
                return -1;
                break;
            }
            break;

        case CIA_OPERATION_ENABLE:
            switch(targetState)
            {
            case CIA_SWITCH_ON_DISABLE:
                aControlWord[4] = 0x0;
                break;
            
            case CIA_READY_SWITCH_ON:
                aControlWord[4] = 0x6;
                break;

            case CIA_SWITCH_ON:
                aControlWord[4] = 0x7;
                break;

            case CIA_QUCIK_STOP:
                aControlWord[4] = 0x2;
                break;

            default:                //其他目标状态不允许跳转
                return -1;
                break;
            }
            break;

        case CIA_QUCIK_STOP:
            if(targetState == CIA_SWITCH_ON_DISABLE)
            {
                aControlWord[4] = 0x0;
            }
            else        //其他目标状态不允许跳转
            {
                return -1;
            }
            break;

        case CIA_FAULT:
            if(targetState == CIA_SWITCH_ON_DISABLE)       //Bit7需要发送0-->1
            {
                if(hControlWord == 0xFF)
                {
                    getMotorInfo(canId, aReadControlWord, sizeof(aReadControlWord), &temp);
                    hControlWord = temp;
                    aControlWord[4] = hControlWord & 0xFF7F;
                }
                else
                {
                    getMotorInfo(canId, aReadControlWord, sizeof(aReadControlWord), &temp);
                    hControlWord = temp;
                    aControlWord[4] = hControlWord | 0x80;
                }
            }
            else
            {
                return -1;
            }
            break;

        case CIA_FAULT_REACTION:
            if(targetState == CIA_FAULT)        //等待自动跳转
            {
                ;
            }
            else
            {
                return -1;
            }
            break;

        default:
            break;
        }

        setMotorInfo(canId, aControlWord, sizeof(aControlWord));
        usleep(10000);
        printf("State:%d-->", curState);
        getMotorInfo(canId, aStateWord, sizeof(aStateWord), &ret);
        curState = getState(ret);
        printf("%d\n", curState);
    }

    return -1;
}

int MotorDrv::useSpeedMode()
{
    unsigned char aMotionMode[] = {0x2F, 0x60, 0x60, 0, 3, 0, 0, 0};
    unsigned char aControlWord[] = {0x2B, 0x40, 0x60, 0, 0, 0, 0, 0};
    unsigned char aspeed[] = {0x23, 0xFF, 0x60, 0, 50, 0, 0, 0};
    unsigned char aStateWord[] = {0x40, 0x41, 0x60, 0, 0, 0, 0, 0};
    int temp;
    float Acc, maxSpeed;

    getMotorInfo(canId, aStateWord, sizeof(aStateWord), &temp);
    curState = getState(temp);
    printf("motor state:%x\n", curState);
    int ret = changeState(curState, CIA_READY_SWITCH_ON);     //先把状态切换到可配置状态
    if(ret != 0)
    {
        printf("Motor state change error\n");
        return -1;
    }

    // getMaxSpeed(&maxSpeed);
    // printf("max speed:%f\n", maxSpeed);
    setMaxSpeed(tMotorAttr.maxSpeed);
    setAcc(tMotorAttr.maxAcc);
    setDec(tMotorAttr.maxDec);
    setQuickStopDec(tMotorAttr.stopDec);

    // getAcc(&Acc);
    // printf("max Acc:%d\n", Acc);

    setMotorInfo(canId, aMotionMode, sizeof(aMotionMode));
    setMotorInfo(canId, aspeed, sizeof(aspeed));

    aControlWord[4] = 7;
    setMotorInfo(canId, aControlWord, sizeof(aControlWord));

    aControlWord[4] = 0xF;
    setMotorInfo(canId, aControlWord, sizeof(aControlWord));

    return 0;
}

//设置加速度，单位RPM/s
int MotorDrv::setAcc(float rpmps)
{
    unsigned char aAccCmd[] = {0x23, 0x83, 0x60, 0, 0, 0, 0, 0};
    int acc = rpmps / 60 * tMotorAttr.encoderResolution * tMotorAttr.reductionRatio;
    printf("acc:%d\n", acc);

    aAccCmd[4] = acc & 0xff;
    aAccCmd[5] = (acc >> 8) & 0xff;
    aAccCmd[6] = (acc >> 16) & 0xff;
    aAccCmd[7] = (acc >> 24) & 0xff;
    return setMotorInfo(canId, aAccCmd, sizeof(aAccCmd));
}

//获取电机加转速设置，单位RPM/s
int MotorDrv::getAcc(float *acc)
{
    unsigned char aSpeedCmd[] = {0x40, 0x83, 0x60, 0, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(canId, aSpeedCmd, sizeof(aSpeedCmd), &res);
    if(ret == 0)
    {
        *acc = res * 60 / tMotorAttr.encoderResolution / tMotorAttr.reductionRatio;
        // printf("speed:%d\t %f\n", res, *acc);
    }
    
    return ret;
}

//设置减速度，单位RPM/s
int MotorDrv::setDec(float rpmps)
{
    unsigned char aDecCmd[] = {0x23, 0x84, 0x60, 0, 0, 0, 0, 0};
    int dec = rpmps / 60 * tMotorAttr.encoderResolution * tMotorAttr.reductionRatio;

    aDecCmd[4] = dec & 0xff;
    aDecCmd[5] = (dec >> 8) & 0xff;
    aDecCmd[6] = (dec >> 16) & 0xff;
    aDecCmd[7] = (dec >> 24) & 0xff;
    return setMotorInfo(canId, aDecCmd, sizeof(aDecCmd));
}

//获取电机减转速设置，单位RPM/s
int MotorDrv::getDec(float *Dec)
{
    unsigned char aSpeedCmd[] = {0x40, 0x84, 0x60, 0, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(canId, aSpeedCmd, sizeof(aSpeedCmd), &res);
    if(ret == 0)
    {
        *Dec = res * 60 / tMotorAttr.encoderResolution / tMotorAttr.reductionRatio;
        // printf("speed:%d\t %f\n", res, *Dec);
    }
    
    return ret;
}

//设置快停减速度，单位RPM/s
int MotorDrv::setQuickStopDec(float rpmps)
{
    unsigned char aQuickStopDec[] = {0x23, 0x85, 0x60, 0, 0, 0, 0, 0};
    int dec = rpmps / 60 * tMotorAttr.encoderResolution * tMotorAttr.reductionRatio;

    aQuickStopDec[4] = dec & 0xff;
    aQuickStopDec[5] = (dec >> 8) & 0xff;
    aQuickStopDec[6] = (dec >> 16) & 0xff;
    aQuickStopDec[7] = (dec >> 24) & 0xff;
    return setMotorInfo(canId, aQuickStopDec, sizeof(aQuickStopDec));
}

//获取电机快停减转速设置，单位RPM/s
int MotorDrv::getQuickStopDec(float *Dec)
{
    unsigned char aQuickStopDec[] = {0x40, 0x85, 0x60, 0, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(canId, aQuickStopDec, sizeof(aQuickStopDec), &res);
    if(ret == 0)
    {
        *Dec = res * 60 / tMotorAttr.encoderResolution * tMotorAttr.reductionRatio;
        // printf("speed:%d\t %f\n", res, *Dec);
    }
    
    return ret;
}

//设置急加度，单位RPM/s^s
int MotorDrv::setJerk(float rpmpss)
{
    unsigned char aJerkCmd[] = {0x23, 0xA4, 0x60, 0x1, 0, 0, 0, 0};
    int jerk = rpmpss / 60 * tMotorAttr.encoderResolution * tMotorAttr.reductionRatio;

    aJerkCmd[4] = jerk & 0xff;
    aJerkCmd[5] = (jerk >> 8) & 0xff;
    aJerkCmd[6] = (jerk >> 16) & 0xff;
    aJerkCmd[7] = (jerk >> 24) & 0xff;
    return setMotorInfo(canId, aJerkCmd, sizeof(aJerkCmd));
}

//获取急加度，单位RPM/s^s
int MotorDrv::getJerk(float *jerk)
{
    unsigned char aJerkCmd[] = {0x40, 0x84, 0x60, 0x1, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(canId, aJerkCmd, sizeof(aJerkCmd), &res);
    if(ret == 0)
    {
        *jerk = res * 60 / tMotorAttr.encoderResolution / tMotorAttr.reductionRatio;
        // printf("speed:%d\t %f\n", res, *Dec);
    }
    
    return ret;
}

//速度模式下设置目标速度，单位RPM
int MotorDrv::setSpeed(float rpm)
{
    unsigned char aSpeedCmd[] = {0x23, 0xFF, 0x60, 0, 0, 0, 0, 0};
    int speed = rpm * tMotorAttr.encoderResolution * tMotorAttr.reductionRatio / 60;

    aSpeedCmd[4] = speed & 0xff;
    aSpeedCmd[5] = (speed >> 8) & 0xff;
    aSpeedCmd[6] = (speed >> 16) & 0xff;
    aSpeedCmd[7] = (speed >> 24) & 0xff;
    return setMotorInfo(canId, aSpeedCmd, sizeof(aSpeedCmd));
}

//获取电机实际转速，单位RPM
int MotorDrv::getSpeed(float *speed)
{
    unsigned char aSpeedCmd[] = {0x40, 0x6C, 0x60, 0, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(canId, aSpeedCmd, sizeof(aSpeedCmd), &res);
    if(ret == 0)
    {
        *speed = res * 60 / tMotorAttr.encoderResolution / tMotorAttr.reductionRatio;
        printf("speed: %f\n", *speed);
        // outFile<< *speed<<",";
    }
    
    return ret;
}

//获取电机最大转速，单位RPM
int MotorDrv::getMaxSpeed(float *maxSpeed)
{
    unsigned char aSpeedCmd[] = {0x40, 0x7F, 0x60, 0, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(canId, aSpeedCmd, sizeof(aSpeedCmd), &res);
    if(ret == 0)
    {
        *maxSpeed = res * 60 / tMotorAttr.encoderResolution / tMotorAttr.reductionRatio;
        printf("max speed:%d\n", res);
    }

    return ret;
}

//获取电机最大转速，单位RPM
int MotorDrv::setMaxSpeed(float maxSpeed)
{
    unsigned char aSpeedCmd[] = {0x23, 0x7F, 0x60, 0, 0, 0, 0, 0};
    int speed = maxSpeed * tMotorAttr.encoderResolution * tMotorAttr.reductionRatio / 60;

    aSpeedCmd[4] = speed & 0xff;
    aSpeedCmd[5] = (speed >> 8) & 0xff;
    aSpeedCmd[6] = (speed >> 16) & 0xff;
    aSpeedCmd[7] = (speed >> 24) & 0xff;
    return setMotorInfo(canId, aSpeedCmd, sizeof(aSpeedCmd));
}

//设置电机的目标位置，单位：度
int MotorDrv::setPosition(float pos)
{
    unsigned char aPositionCmd[] = {0x23, 0x7A, 0x60, 0, 0, 0, 0, 0};
    int temp = pos / 360 * tMotorAttr.encoderResolution;

    aPositionCmd[4] = temp & 0xff;
    aPositionCmd[5] = (temp >> 8) & 0xff;
    aPositionCmd[6] = (temp >> 16) & 0xff;
    aPositionCmd[7] = (temp >> 24) & 0xff;
    return setMotorInfo(canId, aPositionCmd, sizeof(aPositionCmd));
}

//获取电机当前位置，单位：度
int MotorDrv::getPosition(float *pos)
{
    int res, ret;
    unsigned char aPositionCmd[] = {0x40, 0x64, 0x60, 0, 0, 0, 0, 0};

    res = getMotorInfo(canId, aPositionCmd, sizeof(aPositionCmd), &ret);
    if(res == 0)
    {
        *pos = ret / (tMotorAttr.encoderResolution * 1.0f) * 360;
		*pos = *pos/54.4;
        printf("pos:%f\n", *pos);
        // outFile<<"curPos"<<",";
        // outFile<< *pos/54.4 <<",";
    }

    return res;
}

//设置电机的力矩，单位：Nm
int MotorDrv::setTorque(float Nm)
{
    unsigned char aTorqueCmd[] = {0x23, 0x71, 0x60, 0, 0, 0, 0, 0};
    int temp = Nm / tMotorAttr.ratedTorque * 1000;

    aTorqueCmd[4] = temp & 0xff;
    aTorqueCmd[5] = (temp >> 8) & 0xff;
    aTorqueCmd[6] = (temp >> 16) & 0xff;
    aTorqueCmd[7] = (temp >> 24) & 0xff;
    return setMotorInfo(canId, aTorqueCmd, sizeof(aTorqueCmd));
}

//获取电机当前力矩，单位：Nm
int MotorDrv::getTorque(float *Nm)
{
    int res, ret;
    unsigned char aTorqueCmd[] = {0x40, 0x77, 0x60, 0, 0, 0, 0, 0};

    res = getMotorInfo(canId, aTorqueCmd, sizeof(aTorqueCmd), &ret);
    if(res == 0)
    {
        *Nm = ret * tMotorAttr.ratedTorque / 1000;
        printf("torque:%d\n", ret);
        // outFile<< *Nm;
        return *Nm;
    }

    return res;
}

    float tgtPos=0,last_tgtPos=0;
    static float prePos = 0.0f;
    static float curPos = 0.0f;
    float pos;
    float curSpeed,curTor,tgtSpeed;
    float cmdSpeed;
int MotorDrv::test() //run_ctrl_step
{
    static int cnt = 0;
    static float period = 1;        //s  正弦波周期
    static unsigned int cycle = 5000;        //us 执行时间间隔
    float dt = 0.01;
    unsigned char aControlWord[] = {0x2B, 0x40, 0x60, 0, 0, 0, 0, 0};

    getSpeed(&curSpeed);
    getPosition(&curPos);
    getTorque(&curTor);
    // o 
    tgtPos = 270 * sin(2 * 3.141592654f * cnt / period / (1000000.0f / cycle));
    // tgtPos = 0;
    // tgtPos = 100;
    //o/s
    tgtSpeed= (tgtPos-last_tgtPos)/dt;

    #ifdef INFOHEADER
        outFile<<"tgtPos"<<",";
        outFile<<"tgtSpeed"<<",";
    #endif
    outFile<<tgtPos<<",";
    outFile<<tgtSpeed<<",";
    
    double kp=0.2, kd=0, ki=0;
    kp=ini["kp"];
    ki=ini["kp"];
    kd=ini["kd"];
    
    prePos = kp*(tgtPos-curPos)+kd*(tgtSpeed-curSpeed);

    cmdSpeed = prePos * (1000000 / cycle) / 6;  // =16.6
    cmdSpeed = cmdSpeed/6;
    setSpeed(cmdSpeed);
    printf("target Speed:%f  ", tgtSpeed);
    printf("target Pos:%f  ", tgtPos);
    #ifdef INFOHEADER
        outFile<<"curPos"<<",";
        outFile<<"curSpeed"<<",";
        outFile<<"curTor"<<",";
        outFile<<"cmdSpeed"<<",";
    #endif

    outFile<<curPos<<",";
    outFile<<curSpeed<<",";
    outFile<< curTor <<",";
    outFile<< cmdSpeed <<"";
    usleep(cycle);
    last_tgtPos=tgtPos;
    outFile<< std::endl;
    cnt++;
    if(cnt > 5000)
    {
        aControlWord[4] = 6;
        setMotorInfo(canId, aControlWord, sizeof(aControlWord));
        return -1;
    }
    return 0;
}


